#include "kukaforcecontrolhmi.h"
#include "ui_robotview.h"
#include "Package/QCustomplot/qcustomplot.h"
#include "Package/RobotOpenGLView/RoboticOpenGLView.h"

kukaforceguidehmi::kukaforceguidehmi(short Local_Remote){
    LocalRemote = Local_Remote;

    this->ui = new Ui::visualkukaui;
    this->ui->setupUi(this);

    this->UiDataInit();
    this->RobotScopeInit();
    this->ForceScopeInit();

    this->SignalSlotConnect();
}
kukaforceguidehmi::~kukaforceguidehmi(){
    this->UiDataRelease();
    this->ForceScopeRelease();
    this->RobotScopeRelease();
}

short kukaforceguidehmi::getBackendhandle(BackendInterface* p){
    if(LocalRemote == LOCALCONTOLLER){
        this->pbackendinterface = p;
        return 1;
    }
    else
        return 0;
}
short  kukaforceguidehmi::SignalSlotConnect(){
    return 1;
}

short kukaforceguidehmi::UiDataInit(){
    return 1;
}
short kukaforceguidehmi::UiDataUpdate(){
//    if(this->mDataSaveParam.DataSaveMark == false){
//        this->ui->pbDataSave1->setStyleSheet("background-color:gray");
//        this->ui->pbDataSave2->setStyleSheet("background-color:gray");
//    }else{
//        this->ui->pbDataSave1->setStyleSheet("background-color:green");
//        this->ui->pbDataSave2->setStyleSheet("background-color:green");
//    }
    return 1;
}
short kukaforceguidehmi::UiDataRelease(){
    return 1;
}

short kukaforceguidehmi::ForceScopeInit(){
    ui->ForceScopeView->addGraph();
    ui->ForceScopeView->graph(0)->setPen(QPen(Qt::red));
    ui->ForceScopeView->addGraph();
    ui->ForceScopeView->graph(1)->setPen(QPen(Qt::black));
    ui->ForceScopeView->addGraph();
    ui->ForceScopeView->graph(2)->setPen(QPen(Qt::blue));
    ui->ForceScopeView->addGraph();
    ui->ForceScopeView->graph(3)->setPen(QPen(Qt::yellow));
    ui->ForceScopeView->addGraph();
    ui->ForceScopeView->graph(4)->setPen(QPen(Qt::green));
    ui->ForceScopeView->addGraph();
    ui->ForceScopeView->graph(5)->setPen(QPen(Qt::gray));

    ui->ForceScopeView->graph(0)->setAntialiasedFill(true);
    ui->ForceScopeView->yAxis->setRange(-1000,5500);
    ui->ForceScopeView->xAxis->setTickLabelType(QCPAxis::ltDateTime);
    ui->ForceScopeView->xAxis->setDateTimeFormat("h:m:s");
    ui->ForceScopeView->xAxis->setAutoTickStep(false);
    ui->ForceScopeView->xAxis->setTickStep(2);
    ui->ForceScopeView->axisRect()->setupFullAxesBox();
    return NOERROR;
}
short kukaforceguidehmi::ForceScopeUpdate(){
    if(this->LocalRemote == LOCALCONTOLLER){
        if(this->pbackendinterface != nullptr){
            this->pbackendinterface->ReadForceSensorData(mForceSensorData.Data);
        }
    }
    double key = QDateTime::currentDateTime().toMSecsSinceEpoch()/1000.0;
    static double lastPointKey1 = 0;
//    if(this->mStatusMarks.DataScopeViewMark == true){
        if (key-lastPointKey1 > 0.01){
            if(this->ui->FxView->isChecked()){
                ui->ForceScopeView->graph(0)->addData(key, mForceSensorData.Data[0]);
                ui->ForceScopeView->graph(0)->removeDataBefore(key-20);
            }
            else{
                ui->ForceScopeView->graph(0)->addData(key, 0);
                ui->ForceScopeView->graph(0)->removeDataBefore(key-20);
            }
            if(this->ui->FyView->isChecked()){
                ui->ForceScopeView->graph(1)->addData(key, mForceSensorData.Data[1]);
                ui->ForceScopeView->graph(1)->removeDataBefore(key-20);
            }
            else{
                ui->ForceScopeView->graph(1)->addData(key, 0);
                ui->ForceScopeView->graph(1)->removeDataBefore(key-20);
            }
            if(this->ui->FzView->isChecked()){
                ui->ForceScopeView->graph(2)->addData(key, mForceSensorData.Data[2]);
                ui->ForceScopeView->graph(2)->removeDataBefore(key-20);
            }
            else{
                ui->ForceScopeView->graph(2)->addData(key, 0);
                ui->ForceScopeView->graph(2)->removeDataBefore(key-20);
            }
            if(this->ui->MxView->isChecked()){
                ui->ForceScopeView->graph(3)->addData(key, mForceSensorData.Data[3]);
                ui->ForceScopeView->graph(3)->removeDataBefore(key-20);
            }
            else{
                ui->ForceScopeView->graph(3)->addData(key, 0);
                ui->ForceScopeView->graph(3)->removeDataBefore(key-20);
            }
            if(this->ui->MyView->isChecked()){
                ui->ForceScopeView->graph(4)->addData(key, mForceSensorData.Data[4]);
                ui->ForceScopeView->graph(4)->removeDataBefore(key-20);
            }
            else{
                ui->ForceScopeView->graph(4)->addData(key, 0);
                ui->ForceScopeView->graph(4)->removeDataBefore(key-20);
            }
            if(this->ui->MzView->isChecked()){
                ui->ForceScopeView->graph(5)->addData(key, mForceSensorData.Data[5]);
                ui->ForceScopeView->graph(5)->removeDataBefore(key-20);
            }
            else{
                ui->ForceScopeView->graph(5)->addData(key, 0);
                ui->ForceScopeView->graph(5)->removeDataBefore(key-20);
            }
            lastPointKey1 = key;
        }
        ui->ForceScopeView->xAxis->setRange(key+0.25, 20, Qt::AlignRight);
        ui->ForceScopeView->replot();
//    }
    return 1;
}
short kukaforceguidehmi::ForceScopeRelease(){
    return 1;
}


short kukaforceguidehmi::RobotScopeInit(){
    return 1;
}
short kukaforceguidehmi::RobotScopeUpdate(){
    double Joint[6];
    if(this->ui->tabWidget->currentIndex() == 0){
        if(this->LocalRemote == LOCALCONTOLLER){
            if(this->pbackendinterface != nullptr){
                pbackendinterface->getKUKARIst(this->mKUKAData.KUKARIst);
                pbackendinterface->getKUKAAIst(this->mKUKAData.KUKAAIst);
                pbackendinterface->getKUKAATor(this->mKUKAData.KUKAATor);
            }
        }
        else{
        }
        Joint[0] = -this->mKUKAData.KUKAAIst[0];
        Joint[1] =  this->mKUKAData.KUKAAIst[1] + 90;
        Joint[2] =  this->mKUKAData.KUKAAIst[2] - 90;
        Joint[3] = -this->mKUKAData.KUKAAIst[3];
        Joint[4] =  this->mKUKAData.KUKAAIst[4];
        Joint[5] = -this->mKUKAData.KUKAAIst[5];
        this->ui->RobotView->RoboticOpenViewUpdate(Joint);
    }
    return 1;
}
short kukaforceguidehmi::RobotScopeRelease(){
    return 1;
}

void kukaforceguidehmi::RemoteDataRecv(QByteArray Data){
    if(this->LocalRemote == LOCALCONTOLLER){

    }
    else{
        int loopi = 0;
        QXmlStreamReader mQXmlStreamReader;
        mQXmlStreamReader.addData(Data);
        while(!mQXmlStreamReader.atEnd()){
            if(mQXmlStreamReader.isStartElement()){
                if(mQXmlStreamReader.name() == "RIst"){
                    loopi = 0;
                    foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                        QStringRef tempStringRef;
                        QString  tempQString;
                        tempStringRef = attribute.value();
                        tempQString = tempStringRef.toString();
                        mKUKAData.KUKARIst[loopi] = tempQString.toDouble();
                        loopi++;
                    }
                }

                else if(mQXmlStreamReader.name() == "AIst"){
                    loopi = 0;
                    foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                        QStringRef tempStringRef;
                        QString  tempQString;
                        tempStringRef = attribute.value();
                        tempQString = tempStringRef.toString();
                        mKUKAData.KUKAAIst[loopi] = tempQString.toDouble();
                        loopi++;
                    }
                }
                else if(mQXmlStreamReader.name() == "ATIst"){
                    loopi = 0;
                    foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                        QStringRef tempStringRef;
                        QString  tempQString;
                        tempStringRef = attribute.value();
                        tempQString = tempStringRef.toString();
                        mKUKAData.KUKAATor[loopi]= tempQString.toDouble();
                        loopi++;
                    }
                }
                else if(mQXmlStreamReader.name() == "ForceSensor"){
                    loopi = 0;
                    foreach (const QXmlStreamAttribute & attribute, mQXmlStreamReader.attributes()){
                        QStringRef tempStringRef;
                        QString  tempQString;
                        tempStringRef = attribute.value();
                        tempQString = tempStringRef.toString();
                        mForceSensorData.Data[loopi]= tempQString.toDouble();
                        loopi++;
                    }
                }
            }
            mQXmlStreamReader.readNext();
        }
        mQXmlStreamReader.clear();
    }
}
void kukaforceguidehmi::RemoteDataSend(QByteArray& Data){
    if(this->LocalRemote == LOCALCONTOLLER){
        char tempSend[64];
        QString SendString;
        QXmlStreamWriter stream(&SendString);

        memset(tempSend,'\0',strlen(tempSend));
        stream.writeStartElement("TecController");
            stream.writeAttribute("Tec","VisualKUKA");
            stream.writeEmptyElement("RIst");
                sprintf(tempSend,"%f", mKUKAData.KUKARIst[0]);
                    stream.writeAttribute("X",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKARIst[1]);
                    stream.writeAttribute("Y",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKARIst[2]);
                    stream.writeAttribute("Z",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKARIst[3]);
                    stream.writeAttribute("A",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKARIst[4]);
                    stream.writeAttribute("B",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKARIst[5]);
                    stream.writeAttribute("C",tempSend);

            stream.writeEmptyElement("AIst");
                sprintf(tempSend,"%f", mKUKAData.KUKAAIst[0]);
                    stream.writeAttribute("A1",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAAIst[1]);
                    stream.writeAttribute("A2",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAAIst[2]);
                    stream.writeAttribute("A3",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAAIst[3]);
                    stream.writeAttribute("A4",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAAIst[4]);
                    stream.writeAttribute("A5",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAAIst[5]);
                    stream.writeAttribute("A6",tempSend);

            stream.writeEmptyElement("ATIst");
                sprintf(tempSend,"%f", mKUKAData.KUKAATor[0]);
                    stream.writeAttribute("A1T",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAATor[1]);
                    stream.writeAttribute("A2T",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAATor[2]);
                    stream.writeAttribute("A3T",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAATor[3]);
                    stream.writeAttribute("A4T",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAATor[4]);
                    stream.writeAttribute("A5T",tempSend);
                sprintf(tempSend,"%f", mKUKAData.KUKAATor[5]);
                    stream.writeAttribute("A6T",tempSend);
            stream.writeEndElement();

            stream.writeEmptyElement("ForceSensor");
                    sprintf(tempSend,"%f", mForceSensorData.Data[0]);
                        stream.writeAttribute("Fx",tempSend);
                    sprintf(tempSend,"%f", mForceSensorData.Data[1]);
                        stream.writeAttribute("Fy",tempSend);
                    sprintf(tempSend,"%f", mForceSensorData.Data[2]);
                        stream.writeAttribute("Fz",tempSend);
                    sprintf(tempSend,"%f", mForceSensorData.Data[3]);
                        stream.writeAttribute("Mx",tempSend);
                    sprintf(tempSend,"%f", mForceSensorData.Data[4]);
                        stream.writeAttribute("My",tempSend);
                    sprintf(tempSend,"%f", mForceSensorData.Data[5]);
                        stream.writeAttribute("Mz",tempSend);
            stream.writeEndElement();
        stream.writeEndElement();
        Data = SendString.toLocal8Bit();
    }
    else{
    }
}
